
#include <linux/smp.h>
#include <linux/io.h>
#include <linux/of_address.h>
#include <linux/delay.h>

#include <asm/cacheflush.h>
#include <asm/smp_plat.h>
#include <asm/smp_scu.h>
#include <asm/mach/map.h>
#include <mach/platsmp.h>


static void __iomem *ctrl_base;
#include <platsmp_cfg.h>

static inline void cpu_enter_lowpower(void)
{
	unsigned int v;

	flush_cache_all();

	/*
	 * Turn off coherency and L1 D-cache
	 */
	asm volatile(
	"	mrc	p15, 0, %0, c1, c0, 1\n"
	"	bic	%0, %0, #0x40\n"
	"	mcr	p15, 0, %0, c1, c0, 1\n"
	"	mrc	p15, 0, %0, c1, c0, 0\n"
	"	bic	%0, %0, #0x04\n"
	"	mcr	p15, 0, %0, c1, c0, 0\n"
	  : "=&r" (v)
	  : "r" (0)
	  : "cc");
}

#ifdef CONFIG_HOTPLUG_CPU
static void fh_cpu_die(unsigned int cpu)
{
	cpu_enter_lowpower();
	arch_cpu_idle();

	/* We should have never returned from idle */
	panic("cpu %d unexpectedly exit from shutdown\n", cpu);
}

static int fh_cpu_kill(unsigned int cpu)
{
	fh_set_cpu(cpu, false);

	return 1;
}
#endif

static void fh_set_cpu_jump(int cpu, void *jump_addr)
{
	cpu = cpu_logical_map(cpu);
	if (!cpu)
		return;
#ifdef CONFIG_ARCH_FH889X_FH8210
	fh_pmu_set_reg(CPU_START_ADDR(cpu), jump_addr);
#else
	writel_relaxed(virt_to_phys(jump_addr),
					ctrl_base + CPU_START_ADDR(cpu));
#endif
}

static int fh_get_cpu_jump(int cpu)
{
	cpu = cpu_logical_map(cpu);
	if (!cpu)
		return 0;
#ifdef CONFIG_ARCH_FH889X_FH8210
	return fh_pmu_get_reg(CPU_START_ADDR(cpu));
#else
	return readl_relaxed(ctrl_base + CPU_START_ADDR(cpu));
#endif
}

static void __init fh_smp_prepare_cpus(unsigned int max_cpus)
{
#ifndef CONFIG_ARCH_FH889X_FH8210
	void __iomem *base;

	if (!ctrl_base) {
		base = ioremap(CPU_COH_REG_BASE, SZ_4K);
		if (!base) {
			pr_err("ioremap CPU_CTRL_REG_BASE error\n");
			return;
		}
		ctrl_base = base;
	}
#endif
}

static int fh_smp_boot_secondary(unsigned int cpu, struct task_struct *idle)
{
	int cnt = 0;

	dsb();
	do {
		fh_set_cpu_jump(cpu, secondary_startup);
		fh_set_cpu(cpu, true);
		arch_send_wakeup_ipi_mask(cpumask_of(cpu));
		udelay(2000);
		cnt++;
		if (cnt > 25) {
			pr_info("cpu1 bring up failed\n");
			break;
		}
	} while (fh_get_cpu_jump(cpu) != virt_to_phys(secondary_startup));
	return 0;
}

static const struct smp_operations fh_smp_ops __initconst = {
	.smp_prepare_cpus	= fh_smp_prepare_cpus,
	.smp_boot_secondary	= fh_smp_boot_secondary,
#ifdef CONFIG_HOTPLUG_CPU
	.cpu_die		= fh_cpu_die,
	.cpu_kill		= fh_cpu_kill,
#endif
};

#ifdef CONFIG_USE_OF
CPU_METHOD_OF_DECLARE(fhca7_smp, "fh,fh-ca7-smp", &fh_smp_ops);
#else
bool fh_smp_init(void)
{
	int i;

	for (i = 0; i < CONFIG_NR_CPUS; i++) {
		set_cpu_possible(i, true);
		cpu_logical_map(i) = i;
	}
	smp_set_ops(&fh_smp_ops);
	return true;
}
#endif
